/*
 * VelocitySMC.cpp
 *
 *  Created on: 31.01.2013	
 *      Author: Aleix	
 */

#include "rodos.h"
#include "VelocitySMC.h"
#include "../Threads/global.h"
#include "../hardware.h"

#include <stdlib.h>
#include "../Threads/global.h"

extern Systemstate state;
extern PowerDriver motor;

VelocitySMC::VelocitySMC() {
	pre_error_v = 0;
	integral_v = 0;
	output_v = 0;
	S = 0;
	I = 0;
	D = 0;
	// TODO change values for SMC velocity
	state.Gain2Vel = 0.21;
	state.Gain3Vel = 0;
	state.Gain4Vel = 0.0;
}

VelocitySMC::~VelocitySMC() {
	// TODO Auto-generated destructor stub
}

void VelocitySMC::resetSMC(){
	pre_error_v = 0;
	integral_v = 0;
	output_v = 0;
	S = 0;
	I = 0;
	D = 0;
}

void VelocitySMC::run() {
	xprintf("I'm here velocity\n");

	float error = 0;
	float derivative = 0;

	error = state.desVel - state.velKalman;
	if (error > 180) {
		error = error - 360;
	} else if(error < -180){
		error = error + 360;
	}

	// PROPORTIONAL PART (sign)
	if (error <= 0){
		S = -1;
	} else {
		S = 1;
	}
	
	// Avoid integration for too small errors
	if (abs(error) > state.eps_v) {
		integral_v += error * SAMPLETIME_S;
	}
	
	//TODO Derivative of the desired angle!!
	// DERIVATIVE PART
	// derivative = (DesiredVel - pre_DesiredVel) / SAMPLETIME_S;

	S = state.Gain2Vel * S;
	I = state.Gain3Vel * integral_v;
	D = state.Gain4Vel * derivative;
	output_v = S + I + D;

	// Save the error
	pre_error_v = error;

	if(output_v > LIMIT_VOLTAGE){
		output_v = LIMIT_VOLTAGE;
	} else if (output_v < -LIMIT_VOLTAGE){
		output_v = -LIMIT_VOLTAGE;
	}
	state.dutycycle = (float)(output_v / MAX_VOLTAGE * 100);
	motor.setSpeedPercentage(state.dutycycle );
}
